#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import cv2
import numpy as np
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped
def empty(a):
    pass
class image_converter:
    def __init__(self):
        # 创建cv_bridge，声明图像的发布者和订阅者
        self.massage_pub = rospy.Publisher("talker",PoseStamped, queue_size=1)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)
        #self.image_sub = rospy.Subscriber("/iris/usb_cam/image_raw", Image, self.callback)
        #self.depth_image_sub = rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, self.vins_dep_callback)
    def vins_dep_callback(self,data = Image):
        global depth_img
        try:
            depth_img = self.bridge.imgmsg_to_cv2(data, "16UC1")
            depth_img = cv2.resize(depth_img, (1280, 720))
        except CvBridgeError as e:
            print(e)
        x = 360
        y = 640
        fx = 614.9630737304688
        fy = 615.1746215820312
        ppx = 318.2005920410156
        ppy = 241.51356506347656
        real_z = depth_img[x, y] * 0.001  # mm转换m
        real_x = (x - ppx) / fx * real_z
        real_y = (y - ppy) / fy * real_z
        print(real_z)

    def callback(self,data):
        global depth_img
        # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
            cv_image = cv2.resize(cv_image, (1280, 720))
        except CvBridgeError as e:
            print (e)
        cv2.rectangle(cv_image,(640-10,360-10),(640+10,360+10),(0,255,0),2) #画出外接矩形
        cv2.imshow("cv_image",cv_image)
        cv2.waitKey(1)

            #print(res)
        HSV=cv2.cvtColor(cv_image,cv2.COLOR_BGR2HSV)
        print(HSV[360,640])
        try:
        #mypose=PoseStamped()
        #self.massage_pub.publish(mypose) #先发送一个空位置，试探一下，否则第一个包容易丢
        #time.sleep(1)
        
            mypose=PoseStamped()
            mypose.header.frame_id='target' #设置自己的目标
            mypose.pose.position.x=0
            mypose.pose.position.y=0
            mypose.pose.position.z=0
            mypose.pose.orientation.x=0
            mypose.pose.orientation.y=0
            mypose.pose.orientation.z=0
            mypose.pose.orientation.w=1
            self.massage_pub.publish(mypose)
        except CvBridgeError as e:
            print (e)
class image_depth:
    def __init__(self):    
        # 创建cv_bridge，声明图像的发布者和订阅者
        #self.massage_pub = rospy.Publisher("talker",PoseStamped, queue_size=1)
        self.bridge = CvBridge()
        #self.image_sub = rospy.Subscriber("/camera/color/image_raw", Image, self.callback)
        self.depth_image_sub = rospy.Subscriber("/camera/aligned_depth_to_color/image_raw", Image, self.vins_dep_callback)
    def vins_dep_callback(self,data = Image):
        global depth_img
        try:
            depth_img = self.bridge.imgmsg_to_cv2(data, "16UC1")
            depth_img = cv2.resize(depth_img, (1280, 720))
        except CvBridgeError as e:
            print(e)
        x = 360
        y = 640
        fx = 614.9630737304688
        fy = 615.1746215820312
        ppx = 318.2005920410156
        ppy = 241.51356506347656
        real_z = depth_img[x, y] * 0.001  # mm转换m
        real_x = (x - ppx) / fx * real_z
        real_y = (y - ppy) / fy * real_z
        print(real_z)


if __name__ == '__main__':
    try:
        # 初始化ros节点
        rospy.init_node("cv_bridge_test")
        rospy.loginfo("Starting cv_bridge_test node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print ("Shutting down cv_bridge_test node.")
        cv2.destroyAllWindows()
